package sma.iss;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.Iterator;
import java.util.List;

import sma.iss.Position.Origin;
import sma.iss.calc.PathOptimizer;

/**
 * To calculate ideal Yaw angle
 * 
 * @author smarsoll
 * 
 */
public class YawModel extends GeneticModel {

	private static final double MAX_YAW = 7.0;

	public void optimizeYawAngleForMinute(int minute) {
		LOGGER.info("optimizeYawAngleForMinute {} (beta={})", minute, beta);

		currentPreviousRevBga = new double[8];
		Arrays.fill(currentPreviousRevBga, 0.0);
		currentDangerCounter = new double[8];
		Arrays.fill(currentDangerCounter, 0.0);
		currentDangerCounterRev = new int[8];
		Arrays.fill(currentDangerCounterRev, 0);

		minuteToCalc = minute;

		// template position
		nextPos = new Position();
		nextPos.setMinute(minute);

		initBeforeMinuteCalculation();
		currentNbAdcance = nbAdvance;
		currentGen = 0;


		Collection<Position> population = new ArrayList<Position>();

		// some random elements
		population.addAll(generateRandomPositions(100));


		// Some generation 0 children
		// population.addAll(addChildren(population, 18));

		// for each new generation wanted
		for (int i = 1; i <= nbGeneration; i++) {
			currentGen = i;
			if (i != 1) {
				Iterator<Position> iterator = population.iterator();
				Position bestScorePos = iterator.next();
				Position secondScorePos = iterator.next();
				List<Position> plusBeaux = new ArrayList<Position>(population);
				plusBeaux.add(secondScorePos);
				population.addAll(addChildren(bestScorePos, plusBeaux, 8));

				// children between 1st/3rd.
				List<Position> subList = new ArrayList<Position>(population);
				subList = subList.subList(0, Math.min(subList.size() - 1, population.size() / 3));
				population.addAll(addChildren(subList, 7));
			}
			// children between all
			population.addAll(addChildren(population, 7));
			if (i != nbGeneration) {
				population.addAll(addMutant(population, 7));
			}

			// check transition is possible, remove bad
			removeImpossibleTransitions(population, false);
			calculateScores(population);

			// new population is sorted by score (or score * distance best)
			population = cleanPopulation(population);
		}

		Position best = population.iterator().next();

		LOGGER.info("best yaw for beta={} at minute {} is {}", beta, minute, best.tempYaw);
		// nextPos = keepBestScores(population);
		nextPos = null;
		// return result;

	}

	protected Position generateOneRandomPosition(Position parent) {
		Position minMax = new Position(nextPos, Origin.RANDOM, currentGen);
		for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
			double targetSpeed = getMaxSpeed(numRot) * (2.0 * rnd.nextDouble() - 1.0);
			// double targetAcc = getMaxAcc(numRot) * (2.0 * rnd.nextDouble() - 1.0);
			// double[] nextShiftSpeed = calculateNextPos(parent.getSpeed(numRot), targetSpeed, targetAcc,
			// deltaSeconde);
			minMax.setAngle(numRot, 360.0 * rnd.nextDouble());
			minMax.setSpeed(numRot, targetSpeed);
		}
		minMax.tempYaw = MAX_YAW * rnd.nextDouble();
		return minMax;
	}

	/**
	 * Assume all posutions are valid.
	 */
	protected void removeImpossibleTransitions(Collection<Position> population, boolean remove) {
		Iterator<Position> it = population.iterator();
		while (it.hasNext()) {
			Position position = it.next();
			position.transitionOk = true;
		}
		if (population.isEmpty()) {
			LOGGER.error("no more population!!!");
		}
	}

	protected void fillChildWithParents(Position child, Position p1, Position p2) {
		super.fillChildWithParents(child, p1, p2);
		double coef1 = rnd.nextDouble();
		double coef2 = 1.0 - coef1;
		child.tempYaw = p1.tempYaw * coef1 + p2.tempYaw * coef2;
	}

	protected Collection<Position> addMutant(Collection<Position> population, int nbMutant) {
		Collection<Position> result = new ArrayList<Position>(nbMutant);
		List<Position> melange = shufflePopulation(population, nbMutant);
		int nbAdded = 0;
		for (Position parent : melange) {
			Position interPos = new Position(parent, Origin.MUTANT, currentGen);
			boolean doMute = true;
			while (doMute) {
				int numRot = Math.abs(rnd.nextInt()) % 10;
				PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
				double targetSpeed = maxMutantSpeedSegment * (2.0 * rnd.nextDouble() - 1.0);
				// double shift = opt.calculateSimpleShift(lastPos.getSpeed(numRot), targetSpeed, deltaSeconde);
				interPos.setAngle(numRot, 360.0 * rnd.nextDouble());
				interPos.setSpeed(numRot, targetSpeed);
				doMute = rnd.nextBoolean();
			}
			if (rnd.nextBoolean()) {
				interPos.tempYaw = MAX_YAW * rnd.nextDouble();
			} else {
				interPos.tempYaw = parent.tempYaw;
			}
			result.add(interPos);
			nbAdded++;
			if (nbAdded >= nbMutant) {
				break;
			}
		}
		LOGGER.info("nb mutant generated={}", result.size());
		return result;
	}

}
